#include "KalmanFilter.h"
#include "ForegroundExtractor.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
//#include <opencv2/highgui/highgui.hpp>
//#include <opencv2/nonfree/features2d.hpp>

 
cv::Rect builtRectangle(cv::vector<cv::Point>,int ,int ,int ,int );
cv::vector<cv::Point2f> extractDescriptors(cv::Mat,cv::Rect);
int main()
{
	/** settings **/
	int maxCorners = 300; 
	double qualityLevel = 0.05; 
    double minDistance = 5.0; 
    
    float learningrate=0.001;
	int soglia_area=3;
	bool detectShadow=false;
	 

	cv::Mat A=(cv::Mat_<uchar>(3,4)<<1,1,1,1, 2,2,2,2, 3,3,3,3);
	//cv::Mat A=cv::Mat::ones(150,150,CV_8U)*195;
	cv::Mat B=(cv::Mat_<uchar>(3,4)<<1,2,1,1, 1,1,1,1, 1,1,1,1);
	//std::cout<<A<<std::endl;
	//cv::imshow("PROVA",A);
	cv::waitKey(30);
	//system("pause");



	
	std::cout<<MatrixUtility::sum(A)<<std::endl;

	std::cout<<A*2<<std::endl;
	std::cout<<A.mul(B)<<std::endl;

	std::cout<<MatrixUtility::geq(A,2)<<std::endl;

	cv::Vector<cv::Point> punti = MatrixUtility::findGEQ(A,2);
	for(int i=0;i<punti.size();i++)
	{
		std::cout<<punti[i]<<std::endl;
	}
	
	
	std::cout<<MatrixUtility::pow(A,0)<<std::endl;	
	
	//system("pause");
	//return 1;


	std::vector<cv::Point2f> corners; 

	cv::Mat frame,frameBLUR,gray,maskOpenCV;
//	cv::VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\visor_1206627910990_video_1.avi");//visor_1206627910990_video_1  Visor_0000072
	//cv::VideoCapture vc("C:\\Users\\domenico\\Documents\\MATLAB\\motiion-detection\\video\\Bari_giorno.avi");

	cv::VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\pet2001_2.avi");
	/* vc>>frame;
	while(frame.data!=NULL)	
	{				
		cv::imshow("TEST",frame);
	}
	system("pause");
	return 1;*/

	//cv::VideoCapture vc=cv::VideoCapture(0);
	//vc.set(CV_CAP_PROP_FRAME_WIDTH, 160);
    //vc.set(CV_CAP_PROP_FRAME_HEIGHT, 120);
	
	 
	cv::waitKey(10);
	for (int i = 0;i<80;i++){vc>>frame;}  //<<<<<<<<<<<<<<<<<<<---------------------------------------------------- JUMP

	
	//imshow("FR",frame);
	//system("pause");
	ForegroundExtractor FE(learningrate,soglia_area,frame.cols,frame.rows,detectShadow,CV_32F);

	while(frame.data!=NULL)	
	{				
		//cv::resize(frame,frame,cv::Size(frame.cols/2,frame.rows/2));
		cv::blur(frame,frameBLUR,cv::Size(5,5));
		cv::vector<cv::vector<cv::Point>> contourOpenCV;
		//mask = FE.DomenicoMethod(frame); 
		maskOpenCV = FE.OpenCVMethod(frameBLUR); 
		//cv::Mat merged = cv::Mat::zeros(mask.rows,mask.cols,mask.type());
		
	/*	for(int i = 0;i<merged.rows;i++)
		{
			for(int j = 0;j<merged.cols;j++)
			{
				merged.at<float>(i,j)=mask.at<float>(i,j)*maskOpenCV.at<uchar>(i,j);
			}
		}*/



		cv::vector<cv::vector<cv::Point>> contours_tmp,contour;
		int SOGLIA_AREA = 100;
		cv::Mat tmp=ForegroundExtractor::Float2CV_8UC1(maskOpenCV);
		//merged.copyTo(tmp);

	cv::vector<cv::Vec4i> hierarchy;
	cv::findContours(tmp,contours_tmp,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_SIMPLE);

	 
	for(int i = 0;i<contours_tmp.size();i++)
		{
			double area = contourArea(contours_tmp[i]);
			//std::cout<<area<<std::endl;
			if(area>SOGLIA_AREA)
			{				 
				contour.push_back(contours_tmp[i]);
			}
		}

		
		for(int i = 0;i<contour.size();i++)
		{
			cv::Rect rettangolo =  builtRectangle(contour[i],frame.rows,frame.cols,3,3);
			
			if(rettangolo.x>=0)
			{

				cv::vector<cv::Point2f> corners = extractDescriptors(frame, rettangolo);

				cv::rectangle(frame,rettangolo,cv::Scalar(255,0,0));
				
				for(int j=0;j<corners.size();j++)
				{					
					cv::circle(frame,cv::Point(corners[j].x+rettangolo.x,corners[j].y+rettangolo.y),2,cv::Scalar(0,255,0));					
				}

			}
			
		}

		
		 
		

		imshow("OpenCV",maskOpenCV);
		//imshow("Merged",merged);
		//imshow("Domenico",mask);	
		imshow("Frame",frame);	
		/*cv::Mat Mask3D;
		cv::vector<cv::Mat> mm;
		mm.push_back(maskOpenCV);
		mm.push_back(merged.);
		mm.push_back(mask);
		cv::merge(mm,Mask3D);
		imshow("3D",Mask3D);*/	

		cv::waitKey(5);
		//system("pause");
		vc>>frame;
		
	}
	return 1;



	while(frame.data!=NULL)	
	{

		cv::cvtColor(frame,gray,CV_BGR2GRAY);			


		cv::vector<cv::vector<cv::Point>> blobs = FE.extract(gray);
		for (int i = 0;i<blobs.size();i++)
		{
			cv::vector<cv::Point> b=blobs[i];
			for(int j=0;j<b.size()-1;j++)
			{
				cv::line(frame,b[j],b[j+1],cv::Scalar(0,255,0));
			}
		}



	/*	cv::goodFeaturesToTrack(gray,corners,maxCorners,qualityLevel,minDistance);
		for(int i = 0;i<corners.size();i++)
		{
			cv::circle(frame,corners[i],3,cv::Scalar(0,0,255));
		}*/



		cv::imshow("Figure 1",frame);
		cv::waitKey(5);

		vc>>frame;
	}

	return 1;

}


int mainPRIMA()
{
	cv::Mat frame;
	cv::VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\gmm.avi");
	//cv::VideoCapture vc("C:\\tmp\\iccv07.avi");
	
	ForegroundExtractor FE(0.01,200,3,4,false);//rimuovere 3 e 4..sono SBAGLIATI!
	
	KalmanFilter KF(8,0.01,0.001,0.001); 

	KF.A=*(cv::Mat_<float>(8,8)<<1,0,0,0,1,0,0,0,  0,1,0,0,0,1,0,0,  0,0,1,0,0,0,1,0,  0,0,0,1,0,0,0,1, 0,0,0,0,1,0,0,0, 0,0,0,0,0,1,0,0, 0,0,0,0,0,0,1,0, 0,0,0,0,0,0,0,1); 
	KF.Z=*(cv::Mat_<float>(8,1)<<0,0,0,0,0,0,0,0);

	for(;;)
	{
		vc>>frame;
		if(frame.data==NULL)
			return 1;
		
		cv::vector<cv::vector<cv::Point>> contours;
		cv::vector<cv::Point> allcontours;
		contours=FE.extract(frame);
		//std::cout<<contours.size()<<std::endl;
		for(int i = 0;i<contours.size();i++)
		{
			if(contours[i].size()>5)
			{			
				cv::RotatedRect rr =cv::fitEllipse(contours[i]);			
				cv::ellipse(frame,rr,cv::Scalar(0,0,255));	
				
				allcontours.insert(allcontours.end(),contours[i].begin(),contours[i].end());
			}

		}
		
		if(allcontours.size()>2)
		{
			cv::RotatedRect rr =cv::fitEllipse(allcontours);			
			cv::ellipse(frame,rr,cv::Scalar(0,255,0));

			KF.Z=*(cv::Mat_<float>(8,1)<<rr.center.x,rr.center.y,rr.boundingRect().width,rr.boundingRect().height,KF.Z.at<float>(0)-rr.center.x,KF.Z.at<float>(1)-rr.center.y,KF.Z.at<float>(2)-rr.boundingRect().width,KF.Z.at<float>(3)-rr.boundingRect().height);
			KF.filter();
			KF.drawTrack(frame,cv::Point(KF.Z.at<float>(0),KF.Z.at<float>(1)),cv::Point(KF.X.at<float>(0),KF.X.at<float>(1)));
			cv::rectangle(frame,cv::Rect(KF.Z.at<float>(0)-KF.Z.at<float>(2)/2,KF.Z.at<float>(1)-KF.Z.at<float>(3)/2,KF.Z.at<float>(2),KF.Z.at<float>(3)),cv::Scalar(0,255,0));
			cv::rectangle(frame,cv::Rect(KF.X.at<float>(0)-KF.X.at<float>(2)/2,KF.X.at<float>(1)-KF.X.at<float>(3)/2,KF.X.at<float>(2),KF.X.at<float>(3)),cv::Scalar(0,0,255));
			
		}
		cv::imshow("Foreground",frame);
		cv::waitKey(30);
		


	}
	
	return 1;
}

cv::Rect builtRectangle(cv::vector<cv::Point>contour,int maxRows,int maxCols,int minWidth = 1,int minHeight = 1)
{
	if(contour.size()>5)
			{
				int top=0;
				int bottom = 1e10;
				int right = 0;
				int left = 1e10;
				for(int j=0;j<contour.size();j++)
				{
					
					if(top<contour[j].y)
					{
						top = contour[j].y;
					}

					if(bottom>contour[j].y)
					{
						bottom = contour[j].y;
					}

					if(right<contour[j].x)
					{
						right = contour[j].x;
					}

					if(left>contour[j].x)
					{
						left = contour[j].x;
					}
				}//end for

				cv::Rect rettangolo(left,bottom,right-left,top-bottom);

				if(rettangolo.height>minHeight && rettangolo.width>minWidth)
				{
				//	std::cout<<rettangolo.x<<","<<rettangolo.y<<","<<rettangolo.width<<","<<rettangolo.height<<"\n";
					rettangolo.x=MAX(0,rettangolo.x);
					rettangolo.x=MIN(maxCols,rettangolo.x);

					rettangolo.y=MAX(0,rettangolo.y);
					rettangolo.y=MIN(maxRows,rettangolo.y);

					return rettangolo;
					//cv::Mat roi = frame(rettangolo);
					//std::cout<<"xxx 3.2\n";
					//imshow("ROI",roi);
					//cv::waitKey(5);
					//std::cout<<"xxx 3.3\n";
					//system("pause");
					//cv::circle(maskOpenCV,cv::Point(rettangolo.x,rettangolo.y),3,cv::Scalar(255,0,0));
				}//end if
				
	}//end if
	
		return cv::Rect(-1,-1,-1,-1);


}


cv::vector<cv::Point2f> extractDescriptors(cv::Mat frame, cv::Rect rectangleROI)
{
				
				cv::Mat roi_gray;
				cv::Mat roi = frame(rectangleROI);
				cv::cvtColor(roi,roi_gray,CV_BGR2GRAY);

				float qL = 0.01;
				float mD =1;
				int maxCorners=20;
				cv::vector<cv::Point2f> corners;
				cv::goodFeaturesToTrack(roi_gray,corners,maxCorners,qL,mD);

				return corners;
}